using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using BepuPhysics.Collidables;
using BepuUtilities.Collections;
using BepuPhysics.CollisionDetection;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics
{

    /// <summary>
    /// 已分配的静电可碰撞对象的集合。
    /// </summary>
    public class Statics
    {
        /// <summary>
        /// 将静电句柄整数值重映射到静电的实际数组索引。
        /// 后备阵列索引可以响应于高速缓存优化而改变。
        /// </summary>
        public Buffer<int> HandleToIndex;
        /// <summary>
        /// 将静电索引重新映射到其句柄。
        /// </summary>
        public Buffer<StaticHandle> IndexToHandle;
        /// <summary>
        /// 每个静电拥有的一组可碰撞物体。可以直接更改推测边距、连续性设置和形状指数。
        /// 在没有通知可碰撞添加或移除的大阶段的情况下,形状索引不能在指向形状和指向任何形状之间转换,反之亦然。
        /// </summary>
        public Buffer<Collidable> Collidables;

        public Buffer<RigidPose> Poses;
        public IdPool HandlePool;
        protected BufferPool pool;
        public int Count;

        Shapes shapes;
        Bodies bodies;
        internal BroadPhase broadPhase;
        internal IslandAwakener awakener;

        public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096)
        {
            this.pool = pool;
            InternalResize(Math.Max(1, initialCapacity));

            this.shapes = shapes;
            this.bodies = bodies;
            this.broadPhase = broadPhase;

            HandlePool = new IdPool(initialCapacity, pool);
        }

        unsafe void InternalResize(int targetCapacity)
        {
            Debug.Assert(targetCapacity > 0, "Resize is not meant to be used as Dispose. If you want to return everything to the pool, use Dispose instead.");
            // 请注意,我们基于静电容量来确定捆绑包容量。这简化了分配条件
            targetCapacity = BufferPool.GetCapacityForCount<int>(targetCapacity);
            Debug.Assert(Poses.Length != BufferPool.GetCapacityForCount<RigidPoses>(targetCapacity), "Should not try to use internal resize of the result won't change the size.");
            pool.ResizeToAtLeast(ref Poses, targetCapacity, Count);
            pool.ResizeToAtLeast(ref IndexToHandle, targetCapacity, Count);
            pool.ResizeToAtLeast(ref HandleToIndex, targetCapacity, Count);
            pool.ResizeToAtLeast(ref Collidables, targetCapacity, Count);
            // 将复制区域之外的所有索引初始化为-1。
            Unsafe.InitBlockUnaligned(HandleToIndex.Memory + Count, 0xFF, (uint)(sizeof(int) * (HandleToIndex.Length - Count)));
            // 请注意,我们在这里不修改idpool的内部队列大小。我们在添加和显式调用EnsureCapacity、Compact和Resize期间懒于处理这一问题。
            // 除非在角落情况下,否则idpool的内部队列通常与实际的静电大小相差甚远,所以在通常情况下,懒惰可以节省一点空间。
            // 如果用户想要保证零大小,EnsureCapacity为他们提供了这样做的选项。
        }

        /// <summary>
        /// 检查静电句柄当前是否注册到静态集合。
        /// </summary>
        /// <param name="Handle">要检查的句柄。</param>
        /// <return>如果句柄存在于集合中,则为True,否则为False。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool StaticExists(StaticHandle handle)
        {
            if (handle.Value < 0 || handle.Value >= HandleToIndex.Length)
                return false;
            // 负索引将静电句柄标记为未使用。
            return HandleToIndex[handle.Value] >= 0;
        }

        [Conditional("DEBUG")]
        public void ValidateExistingHandle(StaticHandle handle)
        {
            Debug.Assert(StaticExists(handle), "Handle must exist according to the StaticExists test.");
            Debug.Assert(handle.Value >= 0, "Handles must be nonnegative.");
            Debug.Assert(handle.Value < HandleToIndex.Length && HandleToIndex[handle.Value] >= 0 && IndexToHandle[HandleToIndex[handle.Value]].Value == handle.Value,
                "This static handle doesn't seem to exist, or the mappings are out of sync. If a handle exists, both directions should match.");
        }

        struct InactiveBodyCollector : IBreakableForEach<int>
        {
            BroadPhase broadPhase;
            BufferPool pool;
            public QuickList<BodyHandle> InactiveBodyHandles;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public InactiveBodyCollector(BroadPhase broadPhase, BufferPool pool)
            {
                this.pool = pool;
                this.broadPhase = broadPhase;
                InactiveBodyHandles = new QuickList<BodyHandle>(32, pool);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void Dispose()
            {
                InactiveBodyHandles.Dispose(pool);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool LoopBody(int leafIndex)
            {
                ref var leaf = ref broadPhase.staticLeaves[leafIndex];
                if (leaf.Mobility != CollidableMobility.Static)
                {
                    InactiveBodyHandles.Add(leaf.BodyHandle, pool);
                }
                return true;
            }
        }

        void AwakenBodiesInBounds(ref BoundingBox bounds)
        {
            var collector = new InactiveBodyCollector(broadPhase, pool);
            broadPhase.StaticTree.GetOverlaps(bounds, ref collector);
            for (int i = 0; i < collector.InactiveBodyHandles.Count; ++i)
            {
                awakener.AwakenBody(collector.InactiveBodyHandles[i]);
            }
            collector.Dispose();
        }

        unsafe void AwakenBodiesInExistingBounds(ref Collidable collidable)
        {
            Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < broadPhase.StaticTree.LeafCount);
            broadPhase.GetStaticBoundsPointers(collidable.BroadPhaseIndex, out var minPointer, out var maxPointer);
            BoundingBox oldBounds;
            oldBounds.Min = *minPointer;
            oldBounds.Max = *maxPointer;
            AwakenBodiesInBounds(ref oldBounds);
        }

        /// <summary>
        /// 从SET BY索引中删除静电。边界框与移除的静电边界框重叠的任何非活动实体都将被强制激活。
        /// </summary>
        /// <param name="index">要删除的静电的索引。</param>
        public void RemoveAt(int index)
        {
            Debug.Assert(index >= 0 && index < Count);
            ValidateExistingHandle(IndexToHandle[index]);
            var handle = IndexToHandle[index];

            ref var collidable = ref Collidables[index];
            Debug.Assert(collidable.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding.");
            AwakenBodiesInExistingBounds(ref collidable);

            var removedBroadPhaseIndex = collidable.BroadPhaseIndex;
            if (broadPhase.RemoveStaticAt(removedBroadPhaseIndex, out var movedLeaf))
            {
                // 当一片叶子从宽阔的阶段移走时,另一片叶子将移动以取代它在叶子集中的位置。
                // 我们必须更新Collisible->叶索引指针以匹配宽阶段中叶的新位置。
                // 移动的叶子有两种可能的情况：
                // 1)它是一个可碰撞的非活动性物体,
                // 2)静电可碰撞。
                // 我们检索到的可碰撞参照告诉我们它是身体还是静电。
                if (movedLeaf.Mobility == CollidableMobility.Static)
                {
                    // 这是一个可碰撞的静电,不是一个身体。
                    Collidables[HandleToIndex[movedLeaf.StaticHandle.Value]].BroadPhaseIndex = removedBroadPhaseIndex;
                }
                else
                {
                    // 这是一个不活跃的身体。
                    bodies.UpdateCollidableBroadPhaseIndex(movedLeaf.BodyHandle, removedBroadPhaseIndex);
                }
            }

            // 将最后一个静电移入卸下的插槽中。
            // 这确实带来了无序-第二次重载可能会有价值,以保持秩序,但它将需要大量副本。
            // 如果一次执行的添加和删除操作太多,以至于破坏了连续性,则可能更好的做法是仅
            // 在事实之后显式排序,而不是尝试递增地保持邻接性。换句话说,将其作为批处理进行处理。
            --Count;
            bool staticMoved = index < Count;
            if (staticMoved)
            {
                var movedStaticOriginalIndex = Count;
                // 将最后一个元素的内存状态向下复制。
                Poses[index] = Poses[movedStaticOriginalIndex];
                // 请注意,如果您曾经将世界惯性视为"始终更新",则需要将其复制到此处。
                Collidables[index] = Collidables[movedStaticOriginalIndex];
                // 将静电手柄指向新位置。
                var lastHandle = IndexToHandle[movedStaticOriginalIndex];
                HandleToIndex[lastHandle.Value] = index;
                IndexToHandle[index] = lastHandle;
            }
            HandlePool.Return(handle.Value, pool);
            HandleToIndex[handle.Value] = -1;

        }
        /// <summary>
        /// 从集合中删除静电。边界框与移除的静电边界框重叠的任何非活动实体都将被强制激活。
        /// </summary>
        /// <param name="Handle">要删除的静电的句柄。</param>
        public void Remove(StaticHandle handle)
        {
            ValidateExistingHandle(handle);
            var removedIndex = HandleToIndex[handle.Value];
            RemoveAt(removedIndex);
        }

        /// <summary>
        /// 更新静电当前状态的广泛阶段内的界限。
        /// </summary>
        public void UpdateBounds(StaticHandle handle)
        {
            var index = HandleToIndex[handle.Value];
            ref var collidable = ref Collidables[index];
            shapes.UpdateBounds(Poses[index], ref collidable.Shape, out var bodyBounds);
            broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bodyBounds.Min, bodyBounds.Max);
        }

        void ComputeNewBoundsAndAwaken(in RigidPose pose, TypedIndex shape, out BoundingBox bounds)
        {
            Debug.Assert(shape.Exists, "Statics must have a shape.");
            // 注意：这里的最小和最大值以绝对坐标表示,这意味着在位置使用更高精度表示的情况下,这是一个必须更新的点。
            shapes[shape.Type].ComputeBounds(shape.Index, pose, out bounds.Min, out bounds.Max);
            AwakenBodiesInBounds(ref bounds);
        }

        internal void ApplyDescriptionByIndexWithoutBroadPhaseModification(int index, in StaticDescription description, out BoundingBox bounds)
        {
            Poses[index] = description.Pose;
            ref var collidable = ref Collidables[index];
            Debug.Assert(description.Collidable.Shape.Exists, "Static collidables must have a shape. Their only purpose is colliding.");
            collidable.Continuity = description.Collidable.Continuity;
            collidable.SpeculativeMargin = description.Collidable.SpeculativeMargin;
            collidable.Shape = description.Collidable.Shape;

            ComputeNewBoundsAndAwaken(description.Pose, description.Collidable.Shape, out bounds);
        }

        /// <summary>
        /// 向仿真中添加新的静电实体。边界框与新静电重叠的所有非活动实体都被强制激活。
        /// </summary>
        /// <param name="description">要添加的静电的描述。</param>
        /// <return>新静电的句柄。</return>
        public StaticHandle Add(in StaticDescription description)
        {
            if (Count == HandleToIndex.Length)
            {
                Debug.Assert(HandleToIndex.Allocated, "The backing memory of the bodies set should be initialized before use. Did you dispose and then not call EnsureCapacity/Resize?");
                // 超出空间;需要调整大小。
                var newSize = HandleToIndex.Length << 1;
                InternalResize(newSize);
            }
            Debug.Assert(Math.Abs(description.Pose.Orientation.Length() - 1) < 1e-6f, "Orientation should be initialized to a unit length quaternion.");
            var handle = new StaticHandle(HandlePool.Take());
            var index = Count++;
            HandleToIndex[handle.Value] = index;
            IndexToHandle[index] = handle;
            ApplyDescriptionByIndexWithoutBroadPhaseModification(index, description, out var bounds);
            // 这是一个新的添加,所以我们需要将其添加到广义阶段。
            Collidables[index].BroadPhaseIndex = broadPhase.AddStatic(new CollidableReference(handle), ref bounds);
            return handle;
        }

        /// <summary>
        /// 更改静电的形状并更新其在宽阶段中的边界。
        /// </summary>
        /// <param name="Handle">要更改形状的静电的句柄。</param>
        /// <param name="newShape">要用于静电的新形状的索引。</param>
        public void SetShape(StaticHandle handle, TypedIndex newShape)
        {
            ValidateExistingHandle(handle);
            Debug.Assert(newShape.Exists, "Statics must have a shape.");
            var index = HandleToIndex[handle.Value];
            ref var collidable = ref Collidables[index];
            AwakenBodiesInExistingBounds(ref collidable);
            // 注意：这里的最小和最大值以绝对坐标表示,这意味着在位置使用更高精度表示的情况下,这是一个必须更新的点。
            ComputeNewBoundsAndAwaken(Poses[index], newShape, out var bounds);
            broadPhase.UpdateStaticBounds(collidable.BroadPhaseIndex, bounds.Min, bounds.Max);
        }

        /// <summary>
        /// 将新描述应用于现有静电对象。边界框与旧静电或新静电可碰撞重叠的所有非活动实体都强制处于活动状态。
        /// 在宽广的阶段更新了静电的边界。
        /// </summary>
        /// <param name="Handle">要应用描述的静电的句柄。</param>
        /// <param name="description">要应用于静电的描述。</param>
        public unsafe void ApplyDescription(StaticHandle handle, in StaticDescription description)
        {
            ValidateExistingHandle(handle);
            var index = HandleToIndex[handle.Value];
            Debug.Assert(description.Collidable.Shape.Exists, "Static collidables cannot lack a shape. Their only purpose is colliding.");
            // 唤醒旧边界和新边界中的所有身体。可能一直在旧静电上休息的不活跃的身体需要意识到新的环境。
            AwakenBodiesInExistingBounds(ref Collidables[index]);
            ApplyDescriptionByIndexWithoutBroadPhaseModification(index, description, out var bounds);
            // 这适用于现有的静电,所以我们应该在宽泛的阶段修改静电的界限。
            broadPhase.UpdateStaticBounds(Collidables[index].BroadPhaseIndex, bounds.Min, bounds.Max);
        }

        /// <summary>
        /// 获取给定句柄引用的静电的当前说明。
        /// </summary>
        /// <param name="Handle">要查找其说明的静电的句柄。</param>
        /// <param name="description">已收集句柄引用的静电的描述。</param>
        public void GetDescription(StaticHandle handle, out StaticDescription description)
        {
            ValidateExistingHandle(handle);
            var index = HandleToIndex[handle.Value];
            BundleIndexing.GetBundleIndices(index, out var bundleIndex, out var innerIndex);
            description.Pose = Poses[index];
            ref var collidable = ref Collidables[index];
            description.Collidable.Continuity = collidable.Continuity;
            description.Collidable.Shape = collidable.Shape;
            description.Collidable.SpeculativeMargin = collidable.SpeculativeMargin;
        }

        /// <summary>
        /// 通过句柄获取对静电的引用。
        /// </summary>
        /// <param name="Handle">要获取引用的静电的句柄。</param>
        /// <return>对所需静电的引用。</return>
        public StaticReference GetStaticReference(StaticHandle handle)
        {
            ValidateExistingHandle(handle);
            return new StaticReference(handle, this);
        }

        /// <summary>
        /// 从集合中清除所有主体,而不向池返回任何内存。
        /// </summary>
        public unsafe void Clear()
        {
            Count = 0;
            // 清空所有索引句柄映射。
            Unsafe.InitBlockUnaligned(HandleToIndex.Memory, 0xFF, (uint)(sizeof(int) * HandleToIndex.Length));
            HandlePool.Clear();
        }

        /// <summary>
        /// 调整为静电数据分配的跨度的大小。请注意,这是保守的;它永远不会孤立现有对象。
        /// </summary>
        /// <param name="Capacity">目标静电数据容量</param>
        public void Resize(int capacity)
        {
            var targetCapacity = BufferPool.GetCapacityForCount<int>(Math.Max(capacity, Count));
            if (IndexToHandle.Length != targetCapacity)
            {
                InternalResize(targetCapacity);
            }
        }

        /// <summary>
        /// 如果需要容纳目标容量,则增加缓冲区的大小。
        /// </summary>
        /// <param name="Capacity">目标数据容量。</param>
        public void EnsureCapacity(int capacity)
        {
            if (IndexToHandle.Length < capacity)
            {
                InternalResize(capacity);
            }
        }

        /// <summary>
        /// 将所有静电资源返回到用于创建它们的池。
        /// </summary>
        /// <remarks>The object can be reused if it is reinitialized by using EnsureCapacity or Resize.</remarks>
        public void Dispose()
        {
            pool.Return(ref Poses);
            pool.Return(ref HandleToIndex);
            pool.Return(ref IndexToHandle);
            pool.Return(ref Collidables);
            HandlePool.Dispose(pool);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private void GatherPose(ref float targetPositionBase, ref float targetOrientationBase, int targetLaneIndex, int index)
        {
            ref var source = ref Poses[index];
            ref var targetPositionSlot = ref Unsafe.Add(ref targetPositionBase, targetLaneIndex);
            ref var targetOrientationSlot = ref Unsafe.Add(ref targetOrientationBase, targetLaneIndex);
            targetPositionSlot = source.Position.X;
            Unsafe.Add(ref targetPositionSlot, Vector<float>.Count) = source.Position.Y;
            Unsafe.Add(ref targetPositionSlot, 2 * Vector<float>.Count) = source.Position.Z;
            targetOrientationSlot = source.Orientation.X;
            Unsafe.Add(ref targetOrientationSlot, Vector<float>.Count) = source.Orientation.Y;
            Unsafe.Add(ref targetOrientationSlot, 2 * Vector<float>.Count) = source.Orientation.Z;
            Unsafe.Add(ref targetOrientationSlot, 3 * Vector<float>.Count) = source.Orientation.W;
        }

        // 这看起来有点不同,因为它是由AABB计算使用的,而不是约束对。
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void GatherDataForBounds(ref int start, int count, out RigidPoses poses, out Vector<int> shapeIndices, out Vector<float> maximumExpansion)
        {
            poses = default; shapeIndices = default; maximumExpansion = default;
            Debug.Assert(count <= Vector<float>.Count);
            ref var targetPositionBase = ref Unsafe.As<Vector<float>, float>(ref poses.Position.X);
            ref var targetOrientationBase = ref Unsafe.As<Vector<float>, float>(ref poses.Orientation.X);
            ref var targetShapeBase = ref Unsafe.As<Vector<int>, int>(ref shapeIndices);
            ref var targetExpansionBase = ref Unsafe.As<Vector<float>, float>(ref maximumExpansion);
            for (int i = 0; i < count; ++i)
            {
                var index = Unsafe.Add(ref start, i);
                GatherPose(ref targetPositionBase, ref targetOrientationBase, i, index);
                ref var collidable = ref Collidables[index];
                Unsafe.Add(ref targetShapeBase, i) = collidable.Shape.Index;
                // 不完全满意的事实是,这从边界计算中引入了一些逻辑。
                Unsafe.Add(ref targetExpansionBase, i) = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin;
            }
        }



    }
}
